#include "fc_srv.h"

static fc_tlm_t tlm_store;
static struct bcshm_t shm;

static struct ev_loop* loop;
static FC_PLUG_TIMER_t action_timer;
static FC_PLUG_TIMER_t takeoff_timer;
static FC_PLUG_TIMER_t land_timer;

/*
 * Галавная функция сервера
 */
void fc_srv_start(std::string channel_name){
  // Открытие общей памяти
  //struct bcshm_t shm;
  bcshm_open(&shm);


  // Конфигурация клиента
  //Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::CompanionComputer}};
  // Или GCS
  Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}};
  
  // Подключение к FC
  ConnectionResult connection_result = mavsdk.add_any_connection(channel_name);
  assert(connection_result == ConnectionResult::Success);
  
  // Создание объекта FC
  auto system = mavsdk.first_autopilot(3.0);
  assert(system);  

  // Создание объекта Action
  auto telemetry = Telemetry{system.value()};
  
  // Создание объекта Telemetry 
  auto action = Action{system.value()};
  //action = new Action{system.value()};
  
  // Настройка телеметрии
  fc_telemetry_init(telemetry);
  fc_ctl_srv_start(action, telemetry);

  while(true);
}

void fc_telemetry_init(Telemetry& telemetry){

  // Инициализация хранилища телеметрии    
  //fc_tlm_t tlm_store;
  fc_tlm_init(&tlm_store);

  // Установка таймеров получения телеметрии
  telemetry.set_rate_altitude(1.0);
  telemetry.set_rate_gps_info(1.0);
  telemetry.set_rate_actuator_output_status(1.0);
  telemetry.set_rate_velocity_ned(1.0);
  telemetry.set_rate_position(1.0);

  // Запуск обработчиков телеметрии
  telemetry.subscribe_gps_info([&telemetry](Telemetry::GpsInfo gpsinfo) {
    bcshm_write(SHM_FC_TLM, &shm, &tlm_store);
    tlm_store.gps_fix = static_cast<FixType>(telemetry.gps_info().fix_type);
  });
  telemetry.subscribe_flight_mode([](Telemetry::FlightMode flightMode) {
    tlm_store.flight_mode = static_cast<FlightMode>(flightMode);
  });
  telemetry.subscribe_altitude([](Telemetry::Altitude alt) {
    tlm_store.hight = alt.altitude_relative_m;
  });
  telemetry.subscribe_velocity_ned([](Telemetry::VelocityNed vel) {
    tlm_store.speed = vel.down_m_s;
  });
  telemetry.subscribe_actuator_output_status([](Telemetry::ActuatorOutputStatus acts) {
    tlm_store.actuators_count = std::min(static_cast<int>(acts.active), MAX_ACTUATORS_COUNT);
    for(int i = 0; i < tlm_store.actuators_count; ++i)
      tlm_store.actuators[i] = acts.actuator.at(i);
    std::cout << "\n";
    
    /*for(int i = 0; i < acts.active; ++i)
      std::cout << "I: " << i << " " << acts.actuator.at(i) << "\n";*/
  });
}

void fc_ctl_srv_start(Action& action, Telemetry& telemetry){

  /*std::this_thread::sleep_for(std::chrono::milliseconds(10000));
  const Action::Result land_result = action.land();
  while (telemetry.in_air()) {
    std::cout << "Vehicle is landing...\n";
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
  }
  std::cout << "Landed!\n";*/

  loop = ev_loop_new(EVFLAG_NOENV | EVBACKEND_EPOLL);
  ev_init(&action_timer.timer, action_timer_cb);
  action_timer.timer.repeat = FC_ACTION_TIMER_PERIOD;
  action_timer.plug.action = &action;
  action_timer.plug.telemetry = &telemetry;
  
  ev_timer_again(loop, &action_timer.timer);
  ev_run(loop, 0); 
}

void fc_takeoff_action(FC_PLUG_t plug){
  //std::cout << "Takeoff action!!!\n";

  flight_task_t ft;
  bcshm_read(SHM_FLIGHT_TASK, &shm, &ft);

  FCACTION_t fc_action = FCACTION_EMPTY;
  bcshm_write(SHM_FCACTION, &shm, &fc_action);
  //std::cout << "\tHight: " << ft.hight << "\n";

  plug.action->set_takeoff_altitude(ft.hight);

  const Action::Result arm_result = plug.action->arm();
  const Action::Result takeoff_result = plug.action->takeoff();

  //FC_PLUG_TIMER_t takeoff_timer;
  ev_init(&takeoff_timer.timer, takeoff_timer_cb);
  takeoff_timer.timer.repeat = FC_TAKEOFF_TIMER_PERIOD;
  takeoff_timer.plug = plug;

  ev_timer_again(loop, &takeoff_timer.timer);
}

void fc_land_action(FC_PLUG_t plug){
  //std::cout << "Land action!!!\n";

  FCACTION_t fc_action = FCACTION_EMPTY;
  bcshm_write(SHM_FCACTION, &shm, &fc_action);

  ev_timer_stop(loop, &takeoff_timer.timer);

  const Action::Result land_result = plug.action->land();

  ev_init(&land_timer.timer, land_timer_cb);
  land_timer.timer.repeat = FC_LAND_TIMER_PERIOD;
  land_timer.plug = plug;

  ev_timer_again(loop, &land_timer.timer);
}

static void action_timer_cb(struct ev_loop *loop, ev_timer *w, int revents){
  FC_PLUG_t plug = ((FC_PLUG_TIMER_t*)w)->plug;
  //printf("\tAction(FC) timer cb\n");

  FCACTION_t fc_action;
  bcshm_read(SHM_FCACTION, &shm, &fc_action);

  if(fc_action == FCACTION_TAKEOFF){
    fc_takeoff_action(plug);
  }
  else if(fc_action == FCACTION_LAND){
    fc_land_action(plug);
  }

  return;
}

static void takeoff_timer_cb(struct ev_loop *loop, ev_timer *w, int revents){
  FC_PLUG_t plug = ((FC_PLUG_TIMER_t*)w)->plug;
  //printf("\tTakeoff timer cb\n");

  if(plug.telemetry->flight_mode() == Telemetry::FlightMode::Hold){
    //printf("\tTakeoff timer event\n");
    EVENT_t event = EVENT_TOF;
    bcshm_write(SHM_EVENT, &shm, &event);

    ev_timer_stop(EV_A_ w);
  }

  return;
}

static void land_timer_cb(struct ev_loop *loop, ev_timer *w, int revents){
  FC_PLUG_t plug = ((FC_PLUG_TIMER_t*)w)->plug;
  //printf("\tLand timer cb\n");

  if(!plug.telemetry->in_air()){
    //printf("\tLand timer event\n");
    EVENT_t event = EVENT_LAND;
    bcshm_write(SHM_EVENT, &shm, &event);

    ev_timer_stop(EV_A_ w);
  }

  return;
}
